﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Kinematic
{
    public class Kinematic
    {
        public Vector3 Position;
        public float Orientation;
        public Vector3 Velocity;
        public float Rotation;


        float maxSpeed = 20.0f;


        public Kinematic(Vector3 Position, float Orientation)
        {
            this.Position = Position;
            this.Orientation = Orientation;
            Velocity = Vector3.Zero;
            Rotation = 0.0f;
        }

        public Kinematic()
        {
            Position = Vector3.Zero;
            Orientation = 0.0f;
            Velocity = Vector3.Zero;
            Rotation = 0.0f;
        }

        public Kinematic Clone()
        {
            Kinematic Aux = new Kinematic();
            Aux.Orientation = Orientation;
            Aux.Position = Position;
            Aux.Rotation = Rotation;
            Aux.Velocity = Velocity;

            return Aux;
        }

        public void Update(SteeringOutput Steering, float time)
        {
            Position += Velocity * time;
            Orientation += Rotation * time;

            Velocity += Steering.linear * time;

            if (Velocity.Length() > maxSpeed)
            {
                Velocity.Normalize();
                Velocity *= maxSpeed;
            }

            Orientation += Steering.agular * time;
        }

        public Vector3 OrientationAsVector()
        {
            return new Vector3((float)Math.Sin(Orientation), 0.0f, (float)Math.Cos(Orientation));

        }
    }
}
